
/*
  Source: http://www.sparkfun.com/datasheets/Sensors/Imaging/adjd_s311_cr99_example.pde
  
  ADJD-S311 Color Sensor Breakout Example Code
  by: Jim Lindblom
  SparkFun Electronics
  date: 8/9/11
  License: MIT license (http://www.opensource.org/licenses/mit-license.php)  
  This code is slightly modified from that posted on bildr.org's
  excellent tutorial (http://bildr.org/2011/01/adjd-s371-tutorial/), which
  was taken from Marcus' great initial code
  (http://interactive-matter.eu/2008/08/tinkering-with-adjd-s371-q999/).
  Thanks to Adam and Marcus for the initial code! I'd definitely recommend checking out 
  their tutorial/posts.

  the hookup:
  ADJD-S311 Breakout ------------- Arduino
  ----------------------------------------
      LED ---------------------------D9
      3.3V -------------------------3.3V
      GND -------------------------- GND
      SCL -------------------------- A5
      SDA -------------------------- A4
      GND -------------------------- GND  
      SLP --------------------- Not connected
      CLK --------------------- Not connected
*/
#include <Wire.h>  // We use Wire.h to talk I2C to the sensor

// ADJD-S311's I2C address, don't change
#define ADJD_S311_ADDRESS 0x74

#define RED 0
#define GREEN 1
#define BLUE 2
#define CLEAR 3

// ADJD-S311's register list
#define CTRL 0x00
#define CONFIG 0x01

#define CAP_RED 0x06
#define CAP_GREEN 0x07
#define CAP_BLUE 0x08
#define CAP_CLEAR 0x09

#define INT_RED_LO 0xA
#define INT_RED_HI 0xB
#define INT_GREEN_LO 0xC
#define INT_GREEN_HI 0xD
#define INT_BLUE_LO 0xE
#define INT_BLUE_HI 0xF
#define INT_CLEAR_LO 0x10
#define INT_CLEAR_HI 0x11

#define DATA_RED_LO 0x40
#define DATA_RED_HI 0x41
#define DATA_GREEN_LO 0x42
#define DATA_GREEN_HI 0x43
#define DATA_BLUE_LO 0x44
#define DATA_BLUE_HI 0x45
#define DATA_CLEAR_LO 0x46
#define DATA_CLEAR_HI 0x47

#define OFFSET_RED 0x48
#define OFFSET_GREEN 0x49
#define OFFSET_BLUE 0x4A
#define OFFSET_CLEAR 0x4B

#define LED_ON 10


#define GSSR 0x01
#define GOFS 0x02

#define TOFS 0x01

#define ERR 2


// Pin definitions:
int sdaPin = A4;  // serial data, hardwired, can't change
int sclPin = A5;  // serial clock, hardwired, can't change
int ledPin = 9;  // LED light source pin, any unused pin will work

// initial values for integration time registers
unsigned char colorCap[4] = {15, 10, 1, 5};  // values must be between 0 and 15
unsigned int colorInt[4] = {4095, 4095, 4095, 4095};  // max value for these is 4095
unsigned int colorData[4];  // This is where we store the RGB and C data values
signed char colorOffset[4];  // Stores RGB and C offset values

void setup()
{
  pinMode(ledPin, OUTPUT);  // Set the sensor's LED as output
  analogWrite(ledPin, LED_ON);  // Initially turn LED light source on
  
  Serial.begin(9600);
  
  Wire.begin();
  delay(1);  // Wait for ADJD reset sequence
  resetADJD_S311();

  // handshake
  establishContact();
}

void loop()
{
  while(!Serial.available());  // Wait till something's pressed
  char cmd = Serial.read();
  
  switch (cmd)
  {
    case 'D':
      analogWrite(ledPin, 0);
      break;
    case 'L':
      analogWrite(ledPin, LED_ON);
      break;
    case 'M':
      getRGBC();
      sendADJD_S311_RGBCValues();
      break;
    case 'C':
      resetADJD_S311();
      calibrateColor();
      calibrateClear();
      calibrateCapacitors();
      sendADJD_S311_CalibratedValues();
      break;
    case 'o':
      getOffset(false);
      sendADJD_S311_OffsetValues();
      break;
    case 'O':
      getOffset(true);
      sendADJD_S311_OffsetValues();
      break;
    case 'I':
      sendADJD_S311_CalibratedValues();
      break;
  }
  
  Serial.flush();
}

void resetADJD_S311()
{
  colorCap[RED] = 15;
  colorCap[GREEN] = 11;
  colorCap[BLUE] = 5;
  colorCap[CLEAR] = 5;  
  
  colorInt[RED] = 4095;
  colorInt[GREEN] = 4095;
  
  colorInt[BLUE] = 4095;
  colorInt[CLEAR] = 4095;  
  
  initADJD_S311();
}

void sendADJD_S311_RGBCValues()
{
  Serial.print(colorData[RED]);
  Serial.print(',');
  Serial.print(colorData[GREEN]);
  Serial.print(',');
  Serial.print(colorData[BLUE]);
  Serial.print(',');
  Serial.print(colorData[CLEAR]);
  Serial.println();
}

void sendADJD_S311_CalibratedValues()
{
  // capacitor values
  Serial.print(readRegister(CAP_RED), DEC);
  Serial.print(',');
  Serial.print(readRegister(CAP_GREEN), DEC);
  Serial.print(',');
  Serial.print(readRegister(CAP_BLUE), DEC);
  Serial.print(',');
  Serial.print(readRegister(CAP_CLEAR), DEC);
  Serial.println();
  
  // integration values
  Serial.print(readRegisterInt(INT_RED_LO), DEC);
  Serial.print(',');
  Serial.print(readRegisterInt(INT_GREEN_LO), DEC);
  Serial.print(',');
  Serial.print(readRegisterInt(INT_BLUE_LO), DEC);
  Serial.print(',');
  Serial.print(readRegisterInt(INT_CLEAR_LO), DEC);
  Serial.println();
}

void sendADJD_S311_OffsetValues()
{
  Serial.print(colorOffset[RED]);
  Serial.print(',');
  Serial.print(colorOffset[GREEN]);
  Serial.print(',');
  Serial.print(colorOffset[BLUE]);
  Serial.print(',');
  Serial.print(colorOffset[CLEAR]);
  Serial.println();
}

void initADJD_S311()
{ 
  /*sensor gain registers, CAP_...
  to select number of capacitors.
  value must be <= 15 */
  writeRegister(colorCap[RED] & 0xF, CAP_RED);
  writeRegister(colorCap[GREEN] & 0xF, CAP_GREEN);
  writeRegister(colorCap[BLUE] & 0xF, CAP_BLUE);
  writeRegister(colorCap[CLEAR] & 0xF, CAP_CLEAR);

  /* Write sensor gain registers INT_...
  to select integration time 
  value must be <= 4096 */
  writeRegister((unsigned char)colorInt[RED], INT_RED_LO);
  writeRegister((unsigned char)((colorInt[RED] & 0x1FFF) >> 8), INT_RED_HI);
  writeRegister((unsigned char)colorInt[BLUE], INT_BLUE_LO);
  writeRegister((unsigned char)((colorInt[BLUE] & 0x1FFF) >> 8), INT_BLUE_HI);
  writeRegister((unsigned char)colorInt[GREEN], INT_GREEN_LO);
  writeRegister((unsigned char)((colorInt[GREEN] & 0x1FFF) >> 8), INT_GREEN_HI);
  writeRegister((unsigned char)colorInt[CLEAR], INT_CLEAR_LO);
  writeRegister((unsigned char)((colorInt[CLEAR] & 0x1FFF) >> 8), INT_CLEAR_HI);
}

/* 
This function calibrates the clear integration registers of the ADJD-S311.
*/
int calibrateClear()
{
  int gainFound = 0;
  int upperBox = 4096;
  int lowerBox = 0;
  int half;
  
  while (!gainFound)
  {
    half = ( (upperBox-lowerBox) / 2 ) + lowerBox;
    //no further halfing possbile
    if (half == lowerBox)
      gainFound = 1;
    else 
    {
      writeInt(INT_CLEAR_LO, half);
      performMeasurement();
      int halfValue = readRegisterInt(DATA_CLEAR_LO);

      if (halfValue>1000)
        upperBox = half;
      else if (halfValue<1000)
        lowerBox = half;
      else
        gainFound = 1;
    }
  }
  return half;
}

/* 
This function clalibrates the RG and B integration registers.
*/
int calibrateColor()
{
  int red, green, blue;
  int gainFound = 0;
  int upperBox = 4096;
  int lowerBox = 0;
  int half;
  
  while (!gainFound)
  {
    half = ( (upperBox-lowerBox) / 2 ) + lowerBox;
    //no further halfing possbile
    if (half == lowerBox)
    {
      gainFound = 1;
    }
    else
    {
      writeInt(INT_RED_LO, half);
      writeInt(INT_GREEN_LO, half);
      writeInt(INT_BLUE_LO, half);
      
      red = green = blue = 0;
      for (int i=0; i<4 ;i++)
      {
        performMeasurement();
        red   += readRegisterInt(DATA_RED_LO);
        green += readRegisterInt(DATA_GREEN_LO);
        blue  += readRegisterInt(DATA_BLUE_LO);
      }
      red   /= 4;
      green /= 4;
      blue  /= 4;
    
      int halfValue = 0;

      halfValue = max(halfValue, red);
      halfValue = max(halfValue, green);
      halfValue = max(halfValue, blue);

      if (halfValue > 1000)
      {
        upperBox = half;
      }
      else if (halfValue < 1000)
      {
        lowerBox = half;
      }
      else
      {
        gainFound = 1;
      }
    }
  }
  return half;
}

/* 
This function calibrates each of the RGB and C capacitor registers.
*/
void calibrateCapacitors()
{
  int calibrationRed = 0;
  int calibrationBlue = 0;
  int calibrationGreen = 0;
  int calibrated = 0;

  //need to store detect better calibration
  int oldDiff = 5000;

  while (!calibrated)
  {
    // sensor gain setting (Avago app note 5330)
    // CAPs are 4bit (higher value will result in lower output)
    writeRegister(calibrationRed, CAP_RED);
    writeRegister(calibrationGreen, CAP_GREEN);
    writeRegister(calibrationBlue, CAP_BLUE);

    // int colorGain = _calibrateColorGain();
    int colorGain = readRegisterInt(INT_RED_LO);
    writeInt(INT_RED_LO, colorGain);
    writeInt(INT_GREEN_LO, colorGain);
    writeInt(INT_BLUE_LO, colorGain);

    int maxRead = 0;
    
    int minRead = 4096;
    int red   = 0;
    int green = 0;
    int blue  = 0;
    
    for (int i=0; i<4 ;i++)
    {
      performMeasurement();
      red   += readRegisterInt(DATA_RED_LO);
      green += readRegisterInt(DATA_GREEN_LO);
      blue  += readRegisterInt(DATA_BLUE_LO);
    }
    red   /= 4;
    green /= 4;
    blue  /= 4;

    maxRead = max(maxRead, red);
    maxRead = max(maxRead, green);
    maxRead = max(maxRead, blue);

    minRead = min(minRead, red);
    minRead = min(minRead, green);
    minRead = min(minRead, blue);

    int diff = maxRead - minRead;

    if (abs(oldDiff - diff) >= ERR)
    {
      if ((maxRead == red) && (calibrationRed < 15))
        calibrationRed++;
      else if ((maxRead == green) && (calibrationGreen < 15))
        calibrationGreen++;
      else if ((maxRead == blue) && (calibrationBlue < 15))
        calibrationBlue++;
    }
    else
      calibrated = 1;
      
    oldDiff = diff;
  }
}

/*
This function performs the offset reading and stores the offset data into the colorOffset[] array.
You can turn on data trimming by uncommenting out the writing 0x01 to 0x01 code.
*/
void getOffset(boolean trim)
{
  analogWrite(ledPin, 0);  // turn LED off
  delay(10);  // wait a tic
  writeRegister(GOFS, CTRL); // start sensing
  while(readRegister(CTRL) != 0); // waiting for a result
  
  if (trim)
  {
    writeRegister(TOFS, CONFIG);  // set trim
    delay(100);
  }
  
  for (int i=0; i<4; i++)
    colorOffset[i] = (signed char) readRegister(OFFSET_RED + i);
    
  analogWrite(ledPin, LED_ON);
}

/*
Handshake
*/
void establishContact()
{
  while (!Serial.available());
}

/*
This function writes a 12-bit value to the LO and HI integration registers
*/
void writeInt(int address, int gain)
{
  if (gain < 4096) 
  {
    byte msb = gain >> 8;
    byte lsb = gain;

    writeRegister(lsb, address);
    writeRegister(msb, address + 1);
  }
}

/* 
This must be called before reading any of the data registers. This commands the
ADJD-S311 to perform a measurement, and store the data into the data registers.
*/
void performMeasurement()
{  
  writeRegister(GSSR, CTRL); // start sensing
  while(readRegister(CTRL) != 0); // waiting for a result
}

/* 
This function reads all of the ADJD-S311's data registers and stores them into colorData[]. To get the
most up-to-date data make sure you call performMeasurement() before calling this function.
*/
void getRGBC()
{
  performMeasurement();
  
  colorData[RED] = readRegisterInt(DATA_RED_LO);
  colorData[GREEN] = readRegisterInt(DATA_GREEN_LO);
  colorData[BLUE] = readRegisterInt(DATA_BLUE_LO);
  colorData[CLEAR] = readRegisterInt(DATA_CLEAR_LO);
}

/*
I2C functions
Write a byte of data to a specific ADJD-S311 address
*/
void writeRegister(unsigned char data, unsigned char address)
{
  Wire.beginTransmission(ADJD_S311_ADDRESS);
  Wire.write(address);
  Wire.write(data);
  Wire.endTransmission();
}

/*
I2C functions
Read a byte of data from ADJD-S311 address
*/
unsigned char readRegister(unsigned char address)
{
  unsigned char data;
  
  Wire.beginTransmission(ADJD_S311_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  
  Wire.requestFrom(ADJD_S311_ADDRESS, 1);
  while (!Wire.available());  // wait till we can get data
  
  return Wire.read();
}

/*
I2C functions
Write two bytes of data to ADJD-S311 address and addres+1
*/
int readRegisterInt(unsigned char address)
{
  return readRegister(address) + (readRegister(address+1) << 8);
}

